#include <ros/ros.h>
#include<WarbotsMsg/manualCtrlMsg.h>
#include <iostream>
#include <string>
#include <sstream>


using namespace std;


double StrToDbl(string s) {
    double d;
    stringstream ss(s); //turn the string into a stream
    ss >> d; //convert
    return d;
}


int main(int argc, char **argv)
{
   ros::init(argc, argv, "manual_control");
   ros::start();
   ros::Rate loop_rate(20); // Running at 20 Hz
   ros::NodeHandle n;
   ros::Publisher pub;
   string vector;
   char* str;
   str = new char[sizeof vector];
   pub = n.advertise<WarbotsMsg::manualCtrlMsg>("manual_control", 1);
   
   
   while( ros::ok() )
   {
      WarbotsMsg::manualCtrlMsg msg;
      cout << "enter the position vector in the format mag:dir without space:";
      getline(cin, vector);
      cout << "\n";
      if (vector.empty()) {
         msg.mag = 0.0;
         msg.ang = 0.0;
      }
      else
      {
         for (unsigned int i=0; i<= vector.length(); i++)
         {
            str[i] = vector[i];
         }
         msg.mag = atof(strtok(str, ":"));
         msg.ang = atof(strtok(NULL, ""));
      }
      
      cout << "set: mag[ " << msg.mag << " ], " << "ang[ " << msg.ang << " ]" << endl;
      pub.publish(msg);
      ros::spinOnce();
      loop_rate.sleep();
   }

   return 0;
}
